#!/bin/bash

RED='\033[0;31m'
GREEN='\033[0;32m'
BLUE='\033[0;34m'
YELLOW='\033[0;33m'
NC='\033[0m' # 无颜色

ALL_SUCCESS=1

# 动态打印函数
print_with_delay() {
  local text="$1"
  local delay=0.02
  for ((i=0; i<${#text}; i++)); do
    echo -ne "${text:$i:1}"
    sleep $delay
  done
  echo
}

# 打印分隔线
print_separator() {
  echo -e "${YELLOW}--------------------------------------${NC}"
}

# 检查硬件连接
check_hardware() {
  print_with_delay "开始硬件检测..."
  print_separator

  # 1. 打开pcie can 驱动
  rosnode kill /pcie_can/pcie_can_node
  print_with_delay "打开pcie can驱动..."
  if command -v gnome-terminal &> /dev/null; then
    gnome-terminal -- bash -c "source ADS_Proj/ADS_device/pcie-can-ros/install/setup.bash; roslaunch pcie_can_ros can_driver.launch; exec $SHELL"
  elif command -v xterm &> /dev/null; then
    xterm -e "roslaunch pcie_can_ros can_driver.launch; bash"
  else
    echo -e "${RED}未找到可用的终端模拟器，请手动打开终端并运行 roslaunch pcie_can_ros can_driver.launch${NC}"
  fi
  print_separator

  # 2. 检查路由器连接
  print_with_delay "检查路由器连接 (ping 192.168.1.1)..."
  if ping -c 1 192.168.1.1 > /dev/null 2>&1; then
    print_with_delay "路由器连接正常"
  else
    echo -e "${RED}路由器连接异常，请检查路由器的连接或者配置${NC}"
    ALL_SUCCESS = 0
  fi
  print_separator

  # 3. 检查前单线激光连接
  print_with_delay "检查前单线激光连接 (ping 192.168.198.2)..."
  if ping -c 1 192.168.198.2 > /dev/null 2>&1; then
    print_with_delay "前单线激光连接正常"
  else
    echo -e "${RED}前单线激光连接异常，请检查前单线激光的连接${NC}"
    ALL_SUCCESS = 0
  fi
  print_separator

  # 4. 检查后单线激光连接
  print_with_delay "检查后单线激光连接 (ping 192.168.1.199)..."
  if ping -c 1 192.168.1.199 > /dev/null 2>&1; then
    print_with_delay "后单线激光连接正常"
  else
    echo -e "${RED}后单线激光连接异常，请检查后单线激光的连接或者配置${NC}"
    ALL_SUCCESS = 0
  fi
  print_separator

  # 5. 检查北云惯导网口连接
  print_with_delay "检查北云惯导网口连接 (ping 192.168.8.151)..."
  if ping -c 1 192.168.8.151 > /dev/null 2>&1; then
    print_with_delay "北云惯导网口连接正常"
  else
    echo -e "${RED}北云惯导网口连接异常，请检查北云惯导的网口连接${NC}"
    ALL_SUCCESS = 0
  fi
  print_separator

  # 6. 检查多线激光网口连接
  print_with_delay "检查多线激光口连接 (ping 192.168.10.100)..."
  if ping -c 1 192.168.10.100 > /dev/null 2>&1; then
    print_with_delay "多线激光网口连接正常"
  else
    echo -e "${RED}北云惯导网口连接异常，请检查北云惯导的网口连接${NC}"
    ALL_SUCCESS = 0
  fi
  print_separator

  # Function to check CAN connections with timeout
  check_can_topic() {
    local topic="$1"
    local id="$2"
    timeout 5s rostopic echo "$topic" | grep "id: $id" -q
  }

  # 6. 检查底盘CAN接线
  print_with_delay "检查底盘CAN接线 (rostopic echo /pcie_can/pcie_can_node/to_pcie_can1_messages | grep 'id: 689')..."
  if check_can_topic "/pcie_can/pcie_can_node/to_pcie_can1_messages" "689"; then
    print_with_delay "底盘CAN接线正常"
  else
    echo -e "${RED}底盘CAN接线异常，请检查底盘CAN接线${NC}"
    ALL_SUCCESS = 0
  fi
  print_separator

  # 7. 检查超声CAN接线  419429784
  print_with_delay "检查超声CAN接线 (rostopic echo /pcie_can/pcie_can_node/to_pcie_can2_messages | grep 'id: 419429784')..."
  if check_can_topic "/pcie_can/pcie_can_node/to_pcie_can2_messages" "419429784"; then
    print_with_delay "超声CAN接线正常"
  else
    echo -e "${RED}超声CAN接线异常，请检查超声CAN接线${NC}"
    ALL_SUCCESS = 0
  fi
  print_separator

  # 8. 检查云台LAN接线
  print_with_delay "检查云台LAN接线 (ping 192.168.1.68)..."
  if ping -c 1 192.168.1.68 > /dev/null 2>&1; then
    print_with_delay "云台LAN接线正常"
  else
    echo -e "${RED}云台LAN接线异常，请检查云台LAN接线${NC}"
    ALL_SUCCESS = 0
  fi
  print_separator

if [ "$ALL_SUCCESS" -eq 1 ]; then
  echo -e "${GREEN}检测完毕，接线正常${NC}"
else
  echo -e "${RED}检测完毕，接线存在异常${NC}"
fi

}

# 主脚本执行
check_hardware

